
package edu.wpi.first.team1662;

import edu.wpi.first.team1662.commands.ClimberExtend;
import edu.wpi.first.team1662.commands.ClimberRetract;
import edu.wpi.first.team1662.commands.ShooterFire;
import edu.wpi.first.team1662.commands.ShooterReset;
import edu.wpi.first.team1662.commands.CommandBase;
import edu.wpi.first.team1662.commands.FeedDisc;
import edu.wpi.first.team1662.commands.GetDisc;
import edu.wpi.first.team1662.commands.LowerHorn;
import edu.wpi.first.team1662.commands.RaiseHorn;
import edu.wpi.first.team1662.subsystems.Networktables;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.DigitalIOButton;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.networktables2.client.NetworkTableClient;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * This class is the glue that binds the controls on the physical operator
 * interface to the commands and command groups that allow control of the robot.
 */
public class OI {
    private Joystick drivePad = new Joystick(RobotMap.gamePad);
    private Joystick manipulatePad = new Joystick(RobotMap.shooterPad);
    private Button climbButton =  new JoystickButton(manipulatePad, 3);
    private Button shootButton = new JoystickButton(manipulatePad, 1);
    private Button pickup = new JoystickButton(drivePad, 5);
    private Button pickup2 = new JoystickButton(drivePad, 6);
    private Button hornActivate = new JoystickButton(drivePad, 2);
    private Button hornLower = new JoystickButton(drivePad, 4);
    private Button shift = new JoystickButton(drivePad, 1);
    
    //These are for the single controller option
    private Button shoot1 = new JoystickButton(drivePad, 3);
    // Another type of button you can create is a DigitalIOButton, which is
    // a button or switch hooked up to the cypress module. These are useful if
    // you want to build a customized operator interface.
    // Button button = new DigitalIOButton(1);
    
    // There are a few additional built in buttons you can use. Additionally,
    // by subclassing Button you can create custom triggers and bind those to
    // commands the same as any other Button.
    
    //// TRIGGERING COMMANDS WITH BUTTONS
    // Once you have a button, it's trivial to bind it to a button in one of
    // three ways:
    
    // Start the command when the button is pressed and let it run the command
    // until it is finished as determined by it's isFinished method.
    // button.whenPressed(new ExampleCommand());
    
    // Run the command while the button is being held down and interrupt it once
    // the button is released.
    // button.whileHeld(new ExampleCommand());
    
    // Start the command when the button is released  and let it run the command
    // until it is finished as determined by it's isFinished method.
    // button.whenReleased(new ExampleCommand());
    
    /**
    public double getLeftJoyY(){
        return leftJoy.getY();
    }
    
    public double getRightJoyY(){
        return rightJoy.getRawAxis();
    }
    */
    
    public OI(){
        
        boolean controller = false;
        if(controller = false){
            climbButton.whenPressed(new ClimberExtend());
            climbButton.whenReleased(new ClimberRetract());
            shootButton.whenPressed(new ShooterFire());
            shootButton.whenReleased(new ShooterReset());
            pickup.whenPressed(new GetDisc());
            pickup2.whenPressed(new FeedDisc());
            hornActivate.whenPressed(new RaiseHorn());
            hornLower.whenPressed(new LowerHorn());
        }
        else{
            climbButton.whenPressed(new ClimberExtend());
            climbButton.whenReleased(new ClimberRetract());
            pickup.whenPressed(new GetDisc());
            pickup2.whenPressed(new FeedDisc());
            hornActivate.whenPressed(new RaiseHorn());
            hornLower.whenPressed(new LowerHorn());
            shoot1.whenPressed(new ShooterFire());
            shoot1.whenReleased(new ShooterReset());           
        }
    }
      
    public double getRightStick(){
        return drivePad.getRawAxis(5);
    }
    
    public double getLeftStick(){
        return drivePad.getRawAxis(2);
    }
    
    public double getShooterThrottle(){
        return manipulatePad.getRawAxis(2);
    }
    
    public void updateStatus(){
      //  SmartDashboard.putNumber("Left Stick, Axis 2", ());
        

        
    }
}

